#include "sim_agv_command.h"
#include "application/simulation/simulation_engine.h"
#include <QString>
#include <QStringList>

namespace application {
namespace simulation {

SimAgvCommand::SimAgvCommand(
    int agvNumber,
    SimulationEngine* engine,
    std::shared_ptr<kernel::ILogger> logger)
    : m_agvNumber(agvNumber)
    , m_engine(engine)
    , m_logger(logger)
{
}

kernel::Result<void> SimAgvCommand::moveToStationList(
    int agvNumber,
    const std::vector<std::string>& stations,
    const std::string& operation)
{
    if (!m_engine || !m_engine->physicsSimulator()) {
        return kernel::Result<void>::failure(
            kernel::ErrorCode::ServiceNotInitialized,
            QStringLiteral("仿真引擎未初始化"));
    }

    // 构建站点路径字符串用于日志
    QStringList stationList;
    for (const auto& station : stations) {
        stationList.append(QString::fromStdString(station));
    }

    if (m_logger) {
        const QString msg = QStringLiteral("SimAgv %1: moveToStationList(%2)")
                                .arg(agvNumber)
                                .arg(stationList.join(QStringLiteral(" -> ")));
        m_logger->info(msg);
    }

    // 将路径交给物理模拟器
    m_engine->physicsSimulator()->setPath(agvNumber, stations);

    return kernel::Result<void>::success();
}

kernel::Result<void> SimAgvCommand::moveToStation(
    int agvNumber,
    const std::string& sourceId,
    const std::string& targetId)
{
    if (m_logger) {
        const QString msg =
            QStringLiteral("SimAgv %1: moveToStation(%2 -> %3)")
                .arg(agvNumber)
                .arg(QString::fromStdString(sourceId))
                .arg(QString::fromStdString(targetId));
        m_logger->info(msg);
    }

    // 简化为两个站点的路径
    std::vector<std::string> stations = {sourceId, targetId};
    return moveToStationList(agvNumber, stations);
}

kernel::Result<void> SimAgvCommand::cancelTask(int agvNumber)
{
    if (!m_engine || !m_engine->physicsSimulator()) {
        return kernel::Result<void>::failure(
            kernel::ErrorCode::ServiceNotInitialized,
            QStringLiteral("仿真引擎未初始化"));
    }

    if (m_logger) {
        m_logger->info(QStringLiteral("SimAgv %1: cancelTask").arg(agvNumber));
    }

    m_engine->physicsSimulator()->cancelVehicle(agvNumber);

    return kernel::Result<void>::success();
}

kernel::Result<void> SimAgvCommand::pauseTask(int agvNumber)
{
    if (!m_engine || !m_engine->physicsSimulator()) {
        return kernel::Result<void>::failure(
            kernel::ErrorCode::ServiceNotInitialized,
            QStringLiteral("仿真引擎未初始化"));
    }

    if (m_logger) {
        m_logger->info(QStringLiteral("SimAgv %1: pauseTask").arg(agvNumber));
    }

    m_engine->physicsSimulator()->pauseVehicle(agvNumber);

    return kernel::Result<void>::success();
}

kernel::Result<void> SimAgvCommand::resumeTask(int agvNumber)
{
    if (!m_engine || !m_engine->physicsSimulator()) {
        return kernel::Result<void>::failure(
            kernel::ErrorCode::ServiceNotInitialized,
            QStringLiteral("仿真引擎未初始化"));
    }

    if (m_logger) {
        m_logger->info(QStringLiteral("SimAgv %1: resumeTask").arg(agvNumber));
    }

    m_engine->physicsSimulator()->resumeVehicle(agvNumber);

    return kernel::Result<void>::success();
}

kernel::Result<void> SimAgvCommand::controlJack(int agvNumber, bool raise)
{
    if (m_logger) {
        const QString msg = QStringLiteral("SimAgv %1: controlJack(%2)")
                                .arg(agvNumber)
                                .arg(raise ? QStringLiteral("raise")
                                           : QStringLiteral("lower"));
        m_logger->info(msg);
    }

    // 仿真模式下，顶升命令立即成功（无需物理模拟）
    return kernel::Result<void>::success();
}

kernel::Result<void> SimAgvCommand::startCharging(int agvNumber, const std::string& chargePoint)
{
    if (m_logger) {
        const QString msg = QStringLiteral("SimAgv %1: startCharging(%2)")
                                .arg(agvNumber)
                                .arg(QString::fromStdString(chargePoint));
        m_logger->info(msg);
    }

    if (m_engine && m_engine->physicsSimulator()) {
        m_engine->physicsSimulator()->setCharging(agvNumber, true);
    }

    // 仿真模式下，充电命令立即成功
    return kernel::Result<void>::success();
}

kernel::Result<void> SimAgvCommand::stopCharging(int agvNumber)
{
    if (m_logger) {
        m_logger->info(QStringLiteral("SimAgv %1: stopCharging").arg(agvNumber));
    }

    if (m_engine && m_engine->physicsSimulator()) {
        m_engine->physicsSimulator()->setCharging(agvNumber, false);
    }

    return kernel::Result<void>::success();
}

kernel::Result<void> SimAgvCommand::controlDoor(int agvNumber, int doorId, bool open)
{
    if (m_logger) {
        const QString msg = QStringLiteral("SimAgv %1: controlDoor(%2, %3)")
                                .arg(agvNumber)
                                .arg(doorId)
                                .arg(open ? QStringLiteral("open")
                                          : QStringLiteral("close"));
        m_logger->info(msg);
    }

    return kernel::Result<void>::success();
}

kernel::Result<void> SimAgvCommand::switchMap(
    int agvNumber,
    const std::string& mapName,
    const std::string& switchPoint)
{
    if (m_logger) {
        const QString msg = QStringLiteral("SimAgv %1: switchMap(%2, %3)")
                                .arg(agvNumber)
                                .arg(QString::fromStdString(mapName))
                                .arg(QString::fromStdString(switchPoint));
        m_logger->info(msg);
    }

    // 仿真模式下，切换地图立即成功
    return kernel::Result<void>::success();
}

kernel::Result<void> SimAgvCommand::requestCurrentMap(int agvNumber)
{
    if (m_logger) {
        m_logger->debug(
            QStringLiteral("SimAgv %1: requestCurrentMap").arg(agvNumber));
    }

    return kernel::Result<void>::success();
}

} // namespace simulation
} // namespace application
